import rospy
from ackermann_msgs.msg import AckermannDrive

target_x = 2
target_y = 0
v0 = 0
ackermann = AckermannDrive()
ackermann.speed = 0.0
ackermann.steering_angle = 0.0
ackermann_cmd_pub = rospy.Publisher('/tianracer/ackermann_cmd', AckermannDrive, queue_size=5)

@staticmethod
def get_goal_position():
    """
    docstring
    """
    return x, y

@staticmethod
def get_v0(history_distance, T):
    """
    docstring
    """
    if -1 in history_distance:
        return 0
    else:
        v_first_car = (history_distance[-1] - history_distance[0]) / T *1000 + ackermann.speed
        return v_first_car

@staticmethod
def get_command(x, y, v0):
    """
    docstring
    """
    dx = x - target_x
    dy = y - target_y
    d_theda = dy / dx
    if d_theda > 1:
        angle = 0.5
    elif d_theda < -1:
        angle = -0.5
    else:
        angle = d_theda * 0.5
    # the max angle is 0.5
    d_speed = dx * 3 + v0
    if d_speed > 3:
        speed = 3
    elif d_speed < -3:
        speed = -3
    else:
        speed = d_speed
    # the max speed is 3
    return speed, angle

if __name__ == "__main__":T
    rospy.init_node('robot_teleop')
    T = 50
    ros::Rate r(T)
    history_distance = [-1,-1,-1]
    try:
        while not rospy.is_shutdown():
            x, y = get_goal_position()
            history_distance.append(x)
            history_distance.pop(0)
            v0 = get_v0(history_distance, T)
            speed, angle = get_command(x, y, v0)
            ackermann.speed = speed
            ackermann.steering_angle = angle
            ackermann_cmd_pub.publish(ackermann)
            r.sleep()

    except Exception as e:
        print(e)

    finally:
        ackermann = AckermannDrive()
        ackermann.speed = 0.0
        ackermann.steering_angle = 0.0
        ackermann_cmd_pub.publish(ackermann)
